#include "libGeoutils/tool/shape_tool.h"
#include <cmath>

static const double EARTH_RADIUS_KM = 6371.0;
static const double PI = std::acos(-1.0);
static double degreesToRadians(double degrees) 
{
	return degrees * PI / 180.0;
}
static double radiansToDegrees(double radians) 
{
	return radians * 180.0 / PI;
}

namespace geo {
namespace tool {
class ShapeToolPrivate {
public:
	std::string mLastErrorMessage;
};

ShapeTool::ShapeTool()
{
	d = std::make_shared<ShapeToolPrivate>();
}

ShapeTool::~ShapeTool()
{
}

bool ShapeTool::calc_coord_distance(geo::shape::Coord::sptr coord1, geo::shape::Coord::sptr coord2, double& res) const
{
	if (coord1 && coord2) {
		return calc_coord_distance(
			coord1->getLongitude(), coord1->getLatitude(),
			coord2->getLongitude(), coord2->getLatitude(),
			res
		);
	}

	d->mLastErrorMessage = "Error: nullptr input";
	return false;
}

bool ShapeTool::calc_coord_distance(geo::shape::Coord coord1, geo::shape::Coord coord2, double& res) const
{
	return calc_coord_distance(
		coord1.getLongitude(), coord1.getLatitude(),
		coord2.getLongitude(), coord2.getLatitude(),
		res
	);
}

bool ShapeTool::calc_coord_distance(double lon1, double lat1, double lon2, double lat2, double& res) const
{
	double lat1_rad = degreesToRadians(lat1);
	double lon1_rad = degreesToRadians(lon1);
	double lat2_rad = degreesToRadians(lat2);
	double lon2_rad = degreesToRadians(lon2);

	double delta_lat = lat2_rad - lat1_rad;
	double delta_lon = lon2_rad - lon1_rad;

	double a = std::sin(delta_lat / 2.0) * std::sin(delta_lat / 2.0) +
		std::cos(lat1_rad) * std::cos(lat2_rad) *
		std::sin(delta_lon / 2.0) * std::sin(delta_lon / 2.0);

	double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a));

	res = EARTH_RADIUS_KM * c;
	return true;
}

bool ShapeTool::calc_coord_bearing(geo::shape::Coord::sptr coord1, geo::shape::Coord::sptr coord2, double& res) const
{
	if (coord1 && coord2) {
		return calc_coord_distance(
			coord1->getLongitude(), coord1->getLatitude(),
			coord2->getLongitude(), coord2->getLatitude(),
			res
		);
	}

	d->mLastErrorMessage = "Error: nullptr input";
	return false;
}

bool ShapeTool::calc_coord_bearing(geo::shape::Coord coord1, geo::shape::Coord coord2, double& res) const
{
	return calc_coord_bearing(
		coord1.getLongitude(), coord1.getLatitude(),
		coord2.getLongitude(), coord2.getLatitude(),
		res
	);
}

bool ShapeTool::calc_coord_bearing(double lon1, double lat1, double lon2, double lat2, double& res) const
{
	double lat1_rad = degreesToRadians(lat1);
	double lon1_rad = degreesToRadians(lon1);
	double lat2_rad = degreesToRadians(lat2);
	double lon2_rad = degreesToRadians(lon2);
	double delta_lon = lon2_rad - lon1_rad;

	double y = std::sin(delta_lon) * std::cos(lat2_rad);
	double x = std::cos(lat1_rad) * std::sin(lat2_rad) -
		std::sin(lat1_rad) * std::cos(lat2_rad) * std::cos(delta_lon);

	double bearing_rad = std::atan2(y, x);
	double bearing_deg = radiansToDegrees(bearing_rad);
	res = std::fmod(bearing_deg + 360.0, 360.0);
	return true;
}

geo::shape::Coord ShapeTool::calc_destination(geo::shape::Coord coord, double dist_km, double bearing_deg) const
{
	double lat1_rad = degreesToRadians(coord.getLatitude());
	double lon1_rad = degreesToRadians(coord.getLongitude());
	double bearing_rad = degreesToRadians(bearing_deg);

	double angular_distance = dist_km / EARTH_RADIUS_KM;
	double lat2_rad = std::asin(std::sin(lat1_rad) * std::cos(angular_distance) +
		std::cos(lat1_rad) * std::sin(angular_distance) * std::cos(bearing_rad));
	double lon2_rad = lon1_rad + std::atan2(std::sin(bearing_rad) * std::sin(angular_distance) * std::cos(lat1_rad),
		std::cos(angular_distance) - std::sin(lat1_rad) * std::sin(lat2_rad));

	geo::shape::Coord destination;
	destination.setLatitude(radiansToDegrees(lat2_rad));
	double lon_temp = radiansToDegrees(lon2_rad);
	destination.setLongitude(std::fmod(lon_temp + 540.0, 360.0) - 180.0);
	return destination;
}

std::vector<std::pair<geo::shape::Coord, double>> ShapeTool::calculate_circle_points(
	const geo::shape::Coord& center, 
	double radius_km, 
	double start_bearing_deg, 
	int num_points, 
	geo::shape::Rectangle& rect)
{
	double min_lon = DBL_MAX;
	double min_lat = DBL_MAX;
	double max_lon = -DBL_MAX;
	double max_lat = -DBL_MAX;

	std::vector<std::pair<geo::shape::Coord, double>> circle_data;
	if (num_points <= 0) {
		num_points = 8;
	}

	double angle_step = 360.0 / num_points;
	for (int i = 0; i < num_points; ++i) {
		double current_bearing = start_bearing_deg + i * angle_step;
		double standardized_bearing = std::fmod(current_bearing + 360.0, 360.0);
		auto point = calc_destination(center, radius_km, current_bearing);
		circle_data.push_back(std::make_pair(point, standardized_bearing));
		min_lat = std::min(min_lat, point.getLatitude());
		min_lon = std::min(min_lon, point.getLongitude());
		max_lat = std::max(max_lat, point.getLatitude());
		max_lon = std::max(max_lon, point.getLongitude());
	}

	rect = geo::shape::Rectangle(min_lon, max_lon, min_lat, max_lat);
	return circle_data;
}

std::string ShapeTool::getLastErrorMessage() const
{
	return d->mLastErrorMessage;
}

} // namespace tool
} // namespace geo